#include <sxMassSpringMeshState.h>

Sx::Simulation::Recording::MassSpringMeshState::MassSpringMeshState() {
	this->mesh = nullptr;
}

Sx::Simulation::Recording::MassSpringMeshState::~MassSpringMeshState() {
	/* this->mesh is not owned by this class (NO DELETE) */
	this->mesh = nullptr;
}

bool Sx::Simulation::Recording::MassSpringMeshState::save(const std::shared_ptr<MassSpringMesh>& mesh) {
	this->mesh = mesh;

	if ( this->mesh == nullptr ) {
		std::cerr << "[MassSpringMeshState:save] Error: Cannot store state of nullptr mass-spring mesh." << std::endl;
		std::cerr << "  Mesh state remains unmodified." << std::endl;
		return false;
	}
	else return this->storeState();
}

bool Sx::Simulation::Recording::MassSpringMeshState::load() {
	if ( this->mesh == nullptr ) {
		std::cerr << "[MassSpringMeshState:load] Error: Cannot restore state of nullptr mass-spring mesh." << std::endl;
		std::cerr << "  Mesh state remains unmodified." << std::endl;
		return false;
	}
	else return this->loadState();
}

bool Sx::Simulation::Recording::MassSpringMeshState::storeState() {
	if ( this->mesh == nullptr ) {
		std::cerr << "[MassSpringMeshState:storeState] Error: Cannot store state of nullptr Mass-spring mesh." << std::endl;
		return false;
	}

	if ( this->mesh->getPhysicalRepresentation() == nullptr ) {
		std::cerr << "[MassSpringMeshState:storeState] Error: Mass-spring mesh does not have a valid physical representation (nullptr)." << std::endl;
		return false;
	}

	//--------------------------------------------------------------------------
	// References to the mesh arrays that are updated in the simulation. These
	// will be changed when the simulation updates. That is why they are
	// copied to this MSM state.
	//--------------------------------------------------------------------------
	const std::shared_ptr<MassSpringSystem>& massSpringSystem = this->mesh->getPhysicalRepresentation();

	Util::ArrayList<Eigen::Vector3d>& meshVertices = massSpringSystem->getVertices();
	Util::ArrayList<Eigen::Vector3d>& meshVelocities = massSpringSystem->state.velocities;
	Util::ArrayList<Eigen::Vector3d>& meshAccelerations = massSpringSystem->state.accelerations;
	Util::ArrayList<Eigen::Vector3d>& meshExternalForces = massSpringSystem->state.externalForces;
	Util::ArrayList<Eigen::Vector3d>& meshInternalForces = massSpringSystem->state.internalForces;

	//--------------------------------------------------------------------------
	// Check to ensure that all of the arrays of the mesh line up. There should
	// be a fixed number of nodes in the mesh, each with its own velocity,
	// acceleration, and external force.
	//--------------------------------------------------------------------------
	const unsigned int nodeCount = meshVertices.size();
	if ( meshVelocities.size() != nodeCount ) {
		std::cerr << "[MassSpringMeshState:storeState] Error: Mass-spring mesh velocity array size does not match node count." << std::endl;
		return false;
	}

	if ( meshAccelerations.size() != nodeCount ) {
		std::cerr << "[MassSpringMeshState:storeState] Error: Mass-spring mesh acceleration array size does not match node count." << std::endl;
		return false;
	}

	if ( meshExternalForces.size() != nodeCount ) {
		std::cerr << "[MassSpringMeshState:storeState] Error: Mass-spring mesh external forces array size does not match node count." << std::endl;
		return false;
	}

	//--------------------------------------------------------------------------
	// Resize the arrays in the state to the size of the provided data arrays.
	//--------------------------------------------------------------------------
	this->meshStorage.vertices.resize(nodeCount);
	this->meshStorage.state.velocities.resize(nodeCount);
	this->meshStorage.state.accelerations.resize(nodeCount);
	this->meshStorage.state.externalForces.resize(nodeCount);
	this->meshStorage.state.internalForces.resize(nodeCount);

	//--------------------------------------------------------------------------
	// Fast-Copy the data from the mesh to this internal meshState.
	//--------------------------------------------------------------------------
	//std::memcpy(&this->meshStorage.vertices[0], &meshVertices[0], sizeof(Eigen::Vector3d) * nodeCount);
	//std::memcpy(&this->meshStorage.state.velocities[0], &meshVelocities[0], sizeof(Eigen::Vector3d) * nodeCount);
	//std::memcpy(&this->meshStorage.state.accelerations[0], &meshAccelerations[0], sizeof(Eigen::Vector3d) * nodeCount);
	//std::memcpy(&this->meshStorage.state.externalForces[0], &meshExternalForces[0], sizeof(Eigen::Vector3d) * nodeCount);
	//std::memcpy(&this->meshStorage.state.internalForces[0], &meshInternalForces[0], sizeof(Eigen::Vector3d) * nodeCount);

	for ( unsigned int i = 0; i < nodeCount; i++ ) {
		this->meshStorage.vertices[i] = meshVertices[i];
		this->meshStorage.state.velocities[i] = meshVelocities[i];
		this->meshStorage.state.accelerations[i] = meshAccelerations[i];
		this->meshStorage.state.externalForces[i] = meshExternalForces[i];
		this->meshStorage.state.internalForces[i] = meshInternalForces[i];
	}

	return true;
}

bool Sx::Simulation::Recording::MassSpringMeshState::loadState() {
	if ( this->mesh == nullptr ) {
		std::cerr << "[MassSpringMeshState:loadState] Error: Cannot store state of nullptr Mass-spring mesh." << std::endl;
		return false;
	}

	if ( this->mesh->getPhysicalRepresentation() == nullptr ) {
		std::cerr << "[MassSpringMeshState:loadState] Error: Mass-spring mesh does not have a valid physical representation (nullptr)." << std::endl;
		return false;
	}

	std::shared_ptr<MassSpringSystem> massSpringSystem = this->mesh->getPhysicalRepresentation();

	// Storage
	Util::ArrayList<Eigen::Vector3d>& meshStateVertices = this->meshStorage.vertices;
	Util::ArrayList<Eigen::Vector3d>& meshStateVelocities = this->meshStorage.state.velocities;
	Util::ArrayList<Eigen::Vector3d>& meshStateAccelerations = this->meshStorage.state.accelerations;
	Util::ArrayList<Eigen::Vector3d>& meshStateExternalForces = this->meshStorage.state.externalForces;

	// Mesh
	Util::ArrayList<Eigen::Vector3d>& meshVertices = massSpringSystem->getVertices();
	Util::ArrayList<Eigen::Vector3d>& meshVelocities = massSpringSystem->state.velocities;
	Util::ArrayList<Eigen::Vector3d>& meshAccelerations = massSpringSystem->state.accelerations;
	Util::ArrayList<Eigen::Vector3d>& meshExternalForces = massSpringSystem->state.externalForces;

	const unsigned int storedVertexCount = meshStateVertices.size();
	const unsigned int storedVelocityCount = meshStateVelocities.size();
	const unsigned int storedAccelerationCount = meshStateAccelerations.size();
	const unsigned int storedExternalForceCount = meshStateExternalForces.size();

	const unsigned int meshVertexCount = meshVertices.size();
	const unsigned int meshVelocityCount = meshVelocities.size();
	const unsigned int meshAccelerationCount = meshAccelerations.size();
	const unsigned int meshExternalForceCount = meshExternalForces.size();

	if ( storedVertexCount != meshVertexCount ) {
		std::cout << "[MassSpringMeshState:loadState] Error: Vertex count mismatch." << std::endl;
		return false;
	}

	if ( storedVelocityCount != meshVelocityCount ) {
		std::cout << "[MassSpringMeshState:loadState] Error: Velocity count mismatch " << std::endl;
		return false;
	}

	if ( storedAccelerationCount != meshAccelerationCount ) {
		std::cout << "[MassSpringMeshState:loadState] Error: Acceleration count mismatch." << std::endl;
		return false;
	}

	if ( storedExternalForceCount != meshExternalForceCount ) {
		std::cout << "[MassSpringMeshState:loadState] Error: External force count mismatch" << std::endl;
		return false;
	}

	// This doesnt work because it doesnt set the positions in the integrator..
	//std::memcpy(&massSpringSystem->getVertices()[0], &this->meshStorage.vertices[0], sizeof(Eigen::Vector3d) * meshVertexCount);
	//std::memcpy(&massSpringSystem->state.velocities[0], &this->meshStorage.state.velocities[0], sizeof(Eigen::Vector3d) * meshVelocityCount);
	//std::memcpy(&massSpringSystem->state.accelerations[0], &this->meshStorage.state.accelerations[0], sizeof(Eigen::Vector3d) * meshAccelerationCount);
	//std::memcpy(&massSpringSystem->state.externalForces[0], &this->meshStorage.state.externalForces[0], sizeof(Eigen::Vector3d) * meshExternalForceCount);

	for ( unsigned int i = 0; i < meshVertexCount; i++ ) {
		mesh->setNodePosition(i, this->meshStorage.vertices[i]);
		mesh->setNodeVelocity(i, this->meshStorage.state.velocities[i]);
		mesh->setNodeAcceleration(i, this->meshStorage.state.accelerations[i]);
		mesh->setNodeExternalForce(i, this->meshStorage.state.externalForces[i]);
		mesh->setNodeInternalForce(i, this->meshStorage.state.internalForces[i]);
	}

	return true;
}
